// Copyright 2022, Uglyhack

use crate::field_transform;
use geo::Polygon;
use graphics::color::{BLACK, RED};
use graphics::Transformed;
use opengl_graphics::GlGraphics;
use piston::input::RenderArgs;
use std::f64::consts::PI;

pub struct Car {
    x: f64,
    y: f64,
    pub a: f64,   // angle of car
    pub w: f64,   // Angle of "middle" front wheel
    pub top: f64, //The top point ever reached
    pub reverse: bool,
}

impl Car {
    // Car statistics based on audi A8 2018
    const WIDTH: f64 = 1.945;
    const LENGTH: f64 = 5.172;
    const WHEELBASE: f64 = 2.998;
    const WHEEL_FROM_BACK: f64 = 1.185;
    pub const MAX_ANGLE: f64 = 0.4; // based on a minimum turn radius of 6.2m;
    const AB: f64 = Self::LENGTH / 2.0 - Self::WHEEL_FROM_BACK; // AB is distance from center of back axle (b) to center of car (a)
                                                                // In calculation:
                                                                // - B is the center of the vehicle
                                                                // - A is the center of the back axle
                                                                // - C is the point around which the vehicle is turning (every wheel is at a right
                                                                //   angle to C)
    const WHEEL_RADIUS: f64 = 0.356; // Wheel radius, not for calculation only for drawing
    const WHEEL_WIDTH: f64 = 0.155; // Wheel width, not for calculation only for drawing.
                                    // Actual value is 0.255, but looks to thick

    pub fn new() -> Self {
        Self {
            x: -5.,
            y: -1.3,
            a: 0.,
            w: 0.,
            top: 0.,
            reverse: false,
        }
    }

    /// Run a step of simulation
    /// dt in seconds
    pub fn step(&mut self, dt_raw: f64) {
        // Indicate direction
        let dt = if self.reverse { -dt_raw } else { dt_raw };

        if self.w.abs() < 0.001 {
            self.y += dt * self.a.sin();
            self.x += dt * self.a.cos();
        } else {
            if self.w.abs() > Self::MAX_ANGLE {
                self.w = Self::MAX_ANGLE.copysign(self.w);
            }
            let ac = Self::WHEELBASE / self.w.tan(); // Radius is either the minimum radius, or the calculated radius
            let bc = (ac * ac + Self::AB * Self::AB).sqrt().copysign(ac);

            let acb = (Self::AB / ac).atan();
            let turn = dt / ac; // Angle we are moving over the turning cirle

            let (c_x, c_y) = self.turning_point();
            let d_angle = self.a - PI / 2.0 + acb + turn;

            self.x = c_x + bc * d_angle.cos();
            self.y = c_y + bc * d_angle.sin();

            self.a += turn;
        }

        self.top = self
            .corners()
            .iter()
            .map(|p| p.1)
            .fold(self.top, |a, b| a.min(b));
    }

    /// Calculate the coordinates of the axle center, AKA, point A
    pub fn axle_center(&self) -> (f64, f64) {
        (
            self.x - self.a.cos() * Self::AB,
            self.y - self.a.sin() * Self::AB,
        )
    }

    /// Calculate the coordinates of the turning point, AKA point C
    pub fn turning_point(&self) -> (f64, f64) {
        let (x, y) = self.axle_center();
        let ac = Self::WHEELBASE / self.w.tan(); // Radius is either the minimum radius, or the calculated radius
        (
            x + (self.a + PI / 2.0).cos() * ac,
            y + (self.a + PI / 2.0).sin() * ac,
        )
    }

    /// Return the angles for the left and right wheel
    /// (Relative to the car angle)
    /// Only used for drawing, no use for the movement math
    pub fn wheel_angles(&self) -> (f64, f64) {
        // If angle is 0, it should stay 0
        if self.w == 0.0 {
            return (0.0, 0.0);
        }
        // Otherwise we're gonna need RC
        let ac = Car::WHEELBASE / self.w.tan();
        // Now calculate angles, left and right
        // Basically, reverse AC calculation but shifted
        let al = (Car::WHEELBASE / (ac - Car::WIDTH / 2.0)).atan();
        let ar = (Car::WHEELBASE / (ac + Car::WIDTH / 2.0)).atan();
        (al, ar)
    }

    /// Return the coordinates of the four corners of the car
    ///
    /// Cordinates are absolute
    fn corners(&self) -> [(f64, f64); 4] {
        let vl = ((Self::WIDTH / 2.0).powi(2) + (Self::LENGTH / 2.0).powi(2)).sqrt();
        let a = (Self::WIDTH / Self::LENGTH).atan();

        let a0 = self.a - a;
        let x0 = self.x + a0.cos() * vl;
        let y0 = self.y + a0.sin() * vl;

        let x2 = self.x - a0.cos() * vl;
        let y2 = self.y - a0.sin() * vl;

        let a1 = self.a + a;
        let x1 = self.x + a1.cos() * vl;
        let y1 = self.y + a1.sin() * vl;

        let x3 = self.x - a1.cos() * vl;
        let y3 = self.y - a1.sin() * vl;

        [(x0, y0), (x1, y1), (x2, y2), (x3, y3)]
    }

    /// Create a polygon object, which can be useful for intersecting
    pub fn to_polygon(&self) -> Polygon<f64> {
        Polygon::new(self.corners().to_vec().into(), vec![])
    }

    /// Draw the car, using args at the graphics gl
    /// The cordinates are rendered with 0,0 at center of screen
    pub fn draw(&self, zoom: f64, args: &RenderArgs, gl: &mut GlGraphics) {
        use graphics::{ellipse, line_from_to, rectangle};

        let car = rectangle::rectangle_by_corners(
            -Car::LENGTH / 2.0,
            -Car::WIDTH / 2.0,
            Car::LENGTH / 2.0,
            Car::WIDTH / 2.0,
        );

        let (c_x, c_y) = self.turning_point();
        let turning_point = ellipse::circle(c_x, c_y, 0.2);

        gl.draw(args.viewport(), |context, gl| {
            let transform_field = field_transform(zoom, args, &context.transform);

            let transform_car = transform_field
                .trans(self.x, self.y) // move to current car position
                .rot_rad(self.a); // Rotate car
            rectangle(RED, car, transform_car, gl);

            let back_axle_x = -Car::AB;
            let front_axle_x = back_axle_x + Car::WHEELBASE;
            let wheel_r = Car::WHEEL_RADIUS;
            let wheel_width = Car::WHEEL_WIDTH;
            let car_half_width = Car::WIDTH / 2.0; // Car half width
                                                   // Back axle
            line_from_to(
                BLACK,
                0.05,
                [back_axle_x, -car_half_width],
                [back_axle_x, car_half_width],
                transform_car,
                gl,
            );
            // Back wheels
            line_from_to(
                BLACK,
                wheel_width,
                [back_axle_x - wheel_r, -car_half_width + wheel_width],
                [back_axle_x + wheel_r, -car_half_width + wheel_width],
                transform_car,
                gl,
            );
            line_from_to(
                BLACK,
                wheel_width,
                [back_axle_x - wheel_r, car_half_width - wheel_width],
                [back_axle_x + wheel_r, car_half_width - wheel_width],
                transform_car,
                gl,
            );

            // front axle
            line_from_to(
                BLACK,
                0.05,
                [front_axle_x, -car_half_width],
                [front_axle_x, car_half_width],
                transform_car,
                gl,
            );
            // front wheels
            let (lwa, rwa) = self.wheel_angles();
            line_from_to(
                BLACK,
                wheel_width,
                [-wheel_r, 0.0],
                [wheel_r, 0.0],
                transform_car
                    .trans(front_axle_x, car_half_width - wheel_width)
                    .rot_rad(lwa),
                gl,
            );
            line_from_to(
                BLACK,
                wheel_width,
                [-wheel_r, 0.0],
                [wheel_r, 0.0],
                transform_car
                    .trans(front_axle_x, -car_half_width + wheel_width)
                    .rot_rad(rwa),
                gl,
            );

            // turning point
            ellipse(BLACK, turning_point, transform_field, gl);

            // Top line
            let middle_y = args.window_size[1] / 2.0;
            let top = self.top * zoom + middle_y;
            line_from_to(
                RED,
                1.,
                [0., top],
                [args.window_size[0], top],
                context.transform,
                gl,
            );
        });
    }
}
